/*
 *	Created by Adam Stroud
 *	September 9, 2014
 *
 */

#include <iostream>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <stdlib.h>
#include <sstream>

#include "../include/ATMega328p.h"
//#include "../include/MotionStatus.h"

using namespace Robot;

ATMega328p* ATMega328p::m_UniqueInstance = new ATMega328p();


ATMega328p::ATMega328p(){

initialize();

}
ATMega328p::~ATMega328p(){

	fclose(file);
}

void ATMega328p::initialize(){


	inertia_counter= 300; INERTIA_WAIT_CYCLES;

	port_fd = init_serial_input(USB_SERIAL_PORT);

	std::cout<<"\nTo run program properly:"<<std::endl;
	std::cout<<"-Make sure Arduino is on ttyACM1"<<std::endl;
	std::cout<<"-Make sure you init_serial_input(USB_SERIAL_PORT); in the start of main()"<<std::endl;
	std::cout<<"-If it is not, unplug USB and plug it into the other port"<<std::endl;
	std::cout<<"-Do $ sudo chmod 777 /dev/ttyACM*\n"<<std::endl;
	std::cout<<"new: try $ stty -F /dev/ttyUSB0 cs8 9600 ignbrk -brkint -icrnl -imaxbel -opost -onlcr -isig -icanon -iexten -echo -echoe -echok -echoctl -echoke noflsh -ixon -crtscts"<<std::endl;
	std::cout<<"-It is important that the size of the packet sent from the arduino be exacly the size of its primitive data types\n"<<std::endl;

	if (port_fd == -1)  {
		std::cout << "Couldn't open USB port" << std::endl;
	}
}

int ATMega328p::init_serial_input (char * port) {
  int fd = 0;
  struct termios options;

  std::cout << "Made Termios" << std::endl;

  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);

  std::cout << "Opened fd USB port" << std::endl;

  if (fd == -1)
    return fd;
    fcntl(fd, F_SETFL, 0);    // clear all flags on descriptor, enable direct I/O
    tcgetattr(fd, &options);   // read serial port options
    // enable receiver, set 8 bit data, ignore control lines
    options.c_cflag |= (CLOCAL | CREAD | CS8);
    // disable parity generation and 2 stop bits
    options.c_cflag &= ~(PARENB | CSTOPB);
    // set the new port options
    tcsetattr(fd, TCSANOW, &options);
    return fd;
}

void ATMega328p::read_serial_xyz (int fd) {
	read(fd, &sensor_buf, 256);
//	std::cout<<sensor_buf;
}

void ATMega328p::parseXYZ(std::string packet){


//	std::cout<<packet<<std::endl;
	// Find the location of the space character

	//0x0x0x0x0
	int space1 = packet.find("GX");

	//GGGXvalue
	std::string split_after_line_1 = packet.substr(space1+1);
	int space2= split_after_line_1.find("GY");
	std::stringstream(packet.substr(space1+2, space2))>>gyroX;


	//GGGYvalue
	std::string split_after_line_2 = packet.substr(space2+2);
	int space3= split_after_line_2.find("GZ");
	std::stringstream(packet.substr(space2+2, space3))>>gyroY;


	//TODO GGGZvalue
	std::string split_after_line_3 = packet.substr(space3+2);
	int space4= split_after_line_3.find("AX");
	//std::stringstream(packet.substr(space3+2, space4))>>gyroZ;


	//AAAXvalue
	std::string split_after_line_4 = packet.substr(space4+2);
	int space5= split_after_line_4.find("AY");
	std::stringstream(packet.substr(space4+2, space5))>>accelX;


	//AAAYvalue
	std::string split_after_line_5 = packet.substr(space5+1);
	int space6= split_after_line_5.find("AZ");
	std::stringstream(packet.substr(space5+2, space6))>>accelY;


	//TODO AAAZvalue
	std::string split_after_line_6 = packet.substr(space6+1);
	int space7= split_after_line_6.find("CX");
	//std::stringstream(packet.substr(space6+2, space7))>>accelZ;


	//TODO CCCXvalue
	std::string split_after_line_7 = packet.substr(space7+1);
	int space8= split_after_line_7.find("CY");
	//std::stringstream(packet.substr(space7+2, space8))>>cmpsX;


	//TODO CCCYvalue
	std::string split_after_line_8 = packet.substr(space8+1);
	int space9= split_after_line_8.find("CZ");
	//std::stringstream(packet.substr(space8+2, space9))>>cmpsY;


	//TODO CCCZvalue
	std::string split_after_line_9 = packet.substr(space9+1);
	int space10= split_after_line_9.find("0x");
	//std::stringstream(packet.substr(space9+2, space10))>>cmpsY;



	//0x0x0x0x0x0x

if(gyroX !=0 && gyroY !=0 && accelX !=0 && accelY !=0 ){
	std::cout<<"Gyro:\t"<<gyroX<<"\t\t"<<gyroY<<"\tAccel:\t"<<accelX<<"\t\t"<<accelY<<std::endl; //"\t"<<gyroZ<<std::endl;
//	std::cout<<"Accel:\t"<<accelX<<"\t"<<accelY<<std::endl; //<<"\t"<<accelZ<<std::endl;
	//std::cout<<"Cmps:\t"<<cmpsX<<"\t"<<cmpsY<<"\t"<<cmpsZ<<std::endl;
}



}

//int getFB_GYRO(){
//	return gyroX;
//}
//int getRL_GYRO(){
//	return gyroY;
//}
//int getFB_ACCEL(){
//	return accelX;
//}
//int getRL_ACCEL(){
//	return accelY;
//}

void ATMega328p::UpdateInertialMeasurements()
{
	if(inertia_counter<0){

		file = fopen(USB_SERIAL_PORT, "w");
		fprintf(file,"%c",'g');
		fclose(file);
		sleep(.05);
		read_serial_xyz(port_fd);
		parseXYZ(sensor_buf);

		file = fopen(USB_SERIAL_PORT, "w");
		fprintf(file,"%c",'a');
		fclose(file);
		sleep(.05);
		read_serial_xyz(port_fd);
		parseXYZ(sensor_buf);

		file = fopen(USB_SERIAL_PORT, "w");
		fprintf(file,"%c",'c');
		fclose(file);
		sleep(.05);
		read_serial_xyz(port_fd);
		parseXYZ(sensor_buf);

		inertia_counter= INERTIA_WAIT_CYCLES;
	}
	else{
		inertia_counter--;
	}

}
